# include <Servo.h>
#define STOP 0 //停止
#define FORWARD 1 //向前
# define BACKWARD 2//后退
# define TURNLEFT 3//左转
# define TURNRIGHT 4//右转
//马达接口
int leftmotor1=16;
int leftmotor2=17;
int rightmotor1=18;
int rightmotor2=19;
//循迹模块
int trac1=10;
int trac2=11;
int trac3=12;
int trac4=13;

int leftpwm=5;
int rightpwm=6;
Servo ms;//舵机
int inputpin=7;//超声波接受
int outputpin =8;//超声波发送



void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  ms.attach(9);//舵机引脚初始化
  /*
  测速引脚初始化
  */
  pinMode(leftmotor1,OUTPUT);
  pinMode(leftmotor2,OUTPUT);
  pinMode(rightmotor1,OUTPUT);
  pinMode(rightmotor2,OUTPUT);
  pinMode(leftpwm,OUTPUT);
  pinMode(rightpwm,OUTPUT);
  // 循迹模块引脚初始化
  pinMode(trac1,INPUT);
  pinMode(trac2,INPUT);
  pinMode(trac3,INPUT);
  pinMode(trac4,INPUT);
  
  
}

void loop() {
  // put your main code here, to run repeatedly:
  tracing();
}

void tracing(){
  int data[4];
  data[0]=digitalRead(10);
  data[1]=digitalRead(11);
  data[2]=digitalRead(12);
  data[3]=digitalRead(13);
  //左右没检测到则继续前进
  if (!data[0]&&!data[1]&&!data[2]&&!data[3])
  {
    motorRun(FORWARD,200);
  }
  //若右边有检测到，则往右转
  if(data[0]||data[1]){
    motorRun(TURNLEFT,150);
  }
  //若左边有检测到，则往左转
  if(data[2]||data[3]){
    motorRun(TURNRIGHT,150);
  }
  //左右都检测到，则停止
  if(data[0]&&data[3]){
    motorRun(STOP,0);
    while(1);
  }
  Serial.println(data[0]);
  Serial.println("-----");
  Serial.println(data[1]);
  Serial.println("-----");
  Serial.println(data[2]);
  Serial.println("-----");
  Serial.println(data[3]);
  Serial.println("-----");
}
void motorRun(int cmd,int value){
  analogWrite(leftpwm,value);
  analogWrite(rightpwm,value);
  switch(cmd){
    case FORWARD:
      Serial.println("FORWARD");
      digitalWrite(leftmotor1,HIGH);
      digitalWrite(leftmotor2,LOW);
      digitalWrite(rightmotor1,HIGH);
      digitalWrite(rightmotor2,LOW);
      break;
     case BACKWARD:
      Serial.println("BACKWARD");
      digitalWrite(leftmotor1,LOW);
      digitalWrite(leftmotor2,HIGH);
      digitalWrite(rightmotor1,LOW);
      digitalWrite(rightmotor2,HIGH);
      break;
     case TURNLEFT:
      Serial.println("trunleft");
      digitalWrite(leftmotor1,HIGH);
      digitalWrite(leftmotor2,LOW);
      digitalWrite(rightmotor1,LOW);
      digitalWrite(rightmotor2,HIGH);
      break;
     case TURNRIGHT:
      Serial.println("trunright");
      digitalWrite(leftmotor1,LOW);
      digitalWrite(leftmotor2,HIGH);
      digitalWrite(rightmotor1,HIGH);
      digitalWrite(rightmotor2,LOW);
      break;
     default:
      Serial.println("stop");
      digitalWrite(leftmotor1,LOW);
      digitalWrite(leftmotor2,LOW);
      digitalWrite(rightmotor1,LOW);
      digitalWrite(rightmotor2,LOW);
      
  }
}

